
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_gcs.c
  * @author     baiyang
  * @date       2021-12-15
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "ahrs.h"

#include <stdint.h>
#include <stdbool.h>

#include <rthw.h>
#include <rtthread.h>
#include <rtdevice.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
#include <sensor_compass/sensor_compass.h>
#include <sensor_imu/sensor_imu.h>
#include <board_config/borad_config.h>
#include <gcs_mavlink/gcs.h>
#include <rc_channel/rc_channel.h>
/*-----------------------------------macro------------------------------------*/
// id(4 byte) + chan(1 byte) + sysid(1 byte) + compid(1 byte) + msg
#define AHRS_GCS_MSG_HEAD_LEN (sizeof(uint32_t) + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint8_t))
/*----------------------------------typedef-----------------------------------*/
typedef struct {
    uint8_t chan;
    uint8_t sysid;          ///< ID of message sender system/aircraft
    uint8_t compid;         ///< ID of the message sender component

    struct rt_ringbuffer msg_rb;       // id(4 byte) + chan(1 byte) + sysid(1 byte) + compid(1 byte) + msg
    uint8_t msg_rb_pool[1024];

    mavlink_message_t mavlink_msg;

    struct rt_mutex _mutex;
} ahrs_gcs_msg;
/*---------------------------------prototype----------------------------------*/
static void _proc_command(mavlink_command_long_t* command, mavlink_message_t* msg);

static void _gcs_cb(void *parameter);
static void _push_msg(uint32_t msg_id, uint8_t chan, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len);
static uint32_t _get_msg_head();
static void _get_msg_date(uint8_t *msg_buf, uint32_t msg_len);
static bool _should_handle_message(const mavlink_message_t* msg);
static bool _should_handle_command_long(const mavlink_command_long_t* command_long);
static MAV_RESULT _handle_preflight_calibration(const mavlink_command_long_t* packet);
static void _handle_command_ack(const mavlink_message_t* msg);
static MAV_RESULT _handle_command_accelcal_vehicle_pos(const mavlink_command_long_t* packet);
/*----------------------------------variable----------------------------------*/
static ahrs_gcs_msg gcs_msg;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_gcs_init()
{
    rt_ringbuffer_init(&gcs_msg.msg_rb, gcs_msg.msg_rb_pool, sizeof(gcs_msg.msg_rb_pool));

    rt_mutex_init(&gcs_msg._mutex, "ahrs_gcs", RT_IPC_FLAG_FIFO);

    itc_subscribe(ITC_ID(mavlink_message),  _gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message1), _gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message2), _gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message3), _gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message4), _gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message5), _gcs_cb);
    itc_subscribe(ITC_ID(mavlink_message6), _gcs_cb);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_gcs_update()
{
    uint32_t msg_id;
    if (rt_ringbuffer_data_len(&gcs_msg.msg_rb) < AHRS_GCS_MSG_HEAD_LEN) {
        // 没有要处理的数据，直接返回
        return;
    }

    msg_id = _get_msg_head();

    switch (msg_id) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            _get_msg_date((uint8_t *)&command, sizeof(mavlink_command_long_t));

            mavlink_msg_command_long_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &command);
            _proc_command(&command, &gcs_msg.mavlink_msg);
        } break;

        case MAVLINK_MSG_ID_COMMAND_ACK: {
            mavlink_command_ack_t command_ack;
            _get_msg_date((uint8_t *)&command_ack, sizeof(mavlink_command_ack_t));

            mavlink_msg_command_ack_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &command_ack);
            _handle_command_ack(&gcs_msg.mavlink_msg);
            break;
        }

        default: {
            gcs_send_text2(gcs_msg.chan, MAV_SEVERITY_INFO, "[ahrs_gcs_update]: unknown mavlink msg:%u\n", msg_id);
        } break;
    }
}

static void _proc_command(mavlink_command_long_t* command, mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (command->command) {
    case MAV_CMD_DO_START_MAG_CAL:
    case MAV_CMD_DO_ACCEPT_MAG_CAL:
    case MAV_CMD_DO_CANCEL_MAG_CAL: {
        result = sensor_compass_handle_mag_cal_command(command);
        break;
    }

    case MAV_CMD_PREFLIGHT_CALIBRATION:
        result = _handle_preflight_calibration(command);
        break;

    case MAV_CMD_ACCELCAL_VEHICLE_POS:
        result = _handle_command_accelcal_vehicle_pos(command);
        break;

    default:
        break;
    }

    if (MAV_HAVE_PAYLOAD_SPACE(gcs_msg.chan, COMMAND_ACK)) {
        mavlink_msg_command_ack_send(gcs_msg.chan, command->command, result,
                                     0, 0,
                                     msg->sysid,
                                     msg->compid);
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void _gcs_cb(void *parameter)
{
    uitc_mavlink_message *mavlink_msg = (uitc_mavlink_message *)parameter;

    if (!_should_handle_message(mavlink_msg->msg_t)) {
        return;
    }

    switch (mavlink_msg->msg_t->msgid) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            mavlink_msg_command_long_decode(mavlink_msg->msg_t, &command);

            _push_msg(MAVLINK_MSG_ID_COMMAND_LONG, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&command, sizeof(mavlink_command_long_t));
        } break;

        case MAVLINK_MSG_ID_COMMAND_ACK: {
            mavlink_command_ack_t command_ack;
            mavlink_msg_command_ack_decode(mavlink_msg->msg_t, &command_ack);

            _push_msg(MAVLINK_MSG_ID_COMMAND_ACK, mavlink_msg->chan, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&command_ack, sizeof(mavlink_command_ack_t));
            break;
        }

        default: {
            gcs_send_text2(mavlink_msg->chan, MAV_SEVERITY_INFO, "[ahrs_gcs_cb]: unknown mavlink msg:%u\n", mavlink_msg->msg_t->msgid);
        } break;
    }
}

/**
  * @brief       
  * @param[in]   msg_id  
  * @param[in]   msg_buf  
  * @param[in]   msg_len  
  * @param[out]  
  * @retval      
  * @note        
  */
static void _push_msg(uint32_t msg_id, uint8_t chan, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len)
{
    if (rt_ringbuffer_space_len(&gcs_msg.msg_rb) >= msg_len + AHRS_GCS_MSG_HEAD_LEN) {

        rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);

        rt_ringbuffer_put(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, chan);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, sysid);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, compid);
        rt_ringbuffer_put(&gcs_msg.msg_rb, msg_buf, msg_len);

        rt_mutex_release(&gcs_msg._mutex);
    } else {
        gcs_send_text2(chan, MAV_SEVERITY_INFO, "ahrs_gcs miss msg");
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static uint32_t _get_msg_head()
{
    uint32_t msg_id;

    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_get(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.chan);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.sysid);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.compid);
    rt_mutex_release(&gcs_msg._mutex);

    return msg_id;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void _get_msg_date(uint8_t *msg_buf, uint32_t msg_len)
{
    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_get(&gcs_msg.msg_rb, msg_buf, msg_len);
    rt_mutex_release(&gcs_msg._mutex);
}

static bool _should_handle_message(const mavlink_message_t* msg)
{
    bool res = false;

    switch (msg->msgid) {
        case MAVLINK_MSG_ID_COMMAND_LONG:
        {
            mavlink_command_long_t command;
            mavlink_msg_command_long_decode(msg, &command);

            res = _should_handle_command_long(&command);
            break;
        }

        case MAVLINK_MSG_ID_COMMAND_ACK:
            res = true;
            break;
        default:
            break;
    }

    return res;
}

static bool _should_handle_command_long(const mavlink_command_long_t* command_long)
{
    bool res = false;

    switch (command_long->command) {
        case MAV_CMD_DO_START_MAG_CAL:
        case MAV_CMD_DO_ACCEPT_MAG_CAL:
        case MAV_CMD_DO_CANCEL_MAG_CAL:
        case MAV_CMD_PREFLIGHT_CALIBRATION: 
        case MAV_CMD_ACCELCAL_VEHICLE_POS: {
            res = true;
        } break;

        default:
          break;
    }

    return res;
}

static MAV_RESULT _handle_preflight_calibration(const mavlink_command_long_t* packet)
{
    MAV_RESULT ret = MAV_RESULT_FAILED;

    if (brd_get_soft_armed()) {
        // *preflight*, remember?
        gcs_send_text2(gcs_msg.chan, MAV_SEVERITY_NOTICE, "Disarm to allow calibration");
        return MAV_RESULT_FAILED;
    }

    if (math_flt_equal(packet->param1,1.0f)) {
        if (!sensor_imu_gyro_calibrated_ok_all()) {
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_ACCEPTED;
    }

    rcs_set_calibrating(math_flt_positive(packet->param4));

    if (math_flt_equal(packet->param5,1.0f)) {
        // start with gyro calibration
        if (!sensor_imu_gyro_calibrated_ok_all()) {
            return MAV_RESULT_FAILED;
        }
        // start accel cal
        sensor_imu_acal_init();
        sensor_imu_acal_start(gcs_chan(gcs_msg.chan));
        return MAV_RESULT_ACCEPTED;
    }

    return ret;
}

static void _handle_command_ack(const mavlink_message_t* msg)
{
    accel_cal_t accelcal = sensor_imu_get_acal();
    if (accelcal != NULL) {
        accel_cal_handleMessage(accelcal, msg);
    }
}

static MAV_RESULT _handle_command_accelcal_vehicle_pos(const mavlink_command_long_t* packet)
{
    if (sensor_imu_get_acal() == NULL ||
        !accel_cal_gcs_vehicle_position(sensor_imu_get_acal(), packet->param1)) {
        return MAV_RESULT_FAILED;
    }
    return MAV_RESULT_ACCEPTED;
}

/*------------------------------------test------------------------------------*/


